
#include "i2c.h"

#define I2C1_MASTER_ADDRESS    0x30
#define I2C1_GYRO_ADDRESS 0x69//1101001b
#define I2C1_ACCELEROMETER1_ADDRESS 0b00011101//0011000b
#define I2C1_ACCELEROMETER2_ADDRESS 0b00011000//0011000b
#define I2C1_MAGNETOMETER_ADDRESS_READ 0b00111101//0011110b
#define I2C1_MAGNETOMETER_ADDRESS_WRITE 0b00011110

#define GYRO_CTRL_REG1 0x20
#define GYRO_CTRL_REG2 0x21
#define GYRO_CTRL_REG3 0x22
#define GYRO_CTRL_REG4 0x23
#define GYRO_CTRL_REG5 0x24
#define GYRO_TEMPERATURE_REG 0x26
#define GYRO_FIFO_CTRL_REG 0x2E
#define GYRO_WHO_AM_I_REG      0x0F
#define GYRO_STATUS_REG    0x27


#define ACCEL2_CTRL_REG1 0x20
#define ACCEL2_CTRL_REG2 0x21
#define ACCEL2_CTRL_REG3 0x22
#define ACCEL2_CTRL_REG4 0x23
#define ACCEL2_CTRL_REG5 0x24

#define ACCEL2_X_L_A	0x28
#define ACCEL2_X_H_A	0x29
#define ACCEL2_Y_L_A	0x2A
#define ACCEL2_Y_H_A	0x2B
#define ACCEL2_Z_L_A	0x2C
#define ACCEL2_Z_H_A	0x2D

#define GYRO_X_L_A	0x28
#define GYRO_X_H_A	0x29
#define GYRO_Y_L_A	0x2A
#define GYRO_Y_H_A	0x2B
#define GYRO_Z_L_A	0x2C
#define GYRO_Z_H_A	0x2D

#define MAG_CRA_REG 0x00
#define MAG_GAIN_RE 0x01
#define MAG_MODE_RE 0x02
#define MAG_X_L_A	0x03
#define MAG_X_H_A	0x04
#define MAG_Y_L_A	0x05
#define MAG_Y_H_A	0x06
#define MAG_Z_L_A	0x07
#define MAG_Z_H_A	0x08


static enum updateType {ACCEL2X1, ACCEL2X2, ACCEL2Y1, ACCEL2Y2, ACCEL2Z1, ACCEL2Z2, GYRO1X1, GYRO1X2, GYRO1Y1, GYRO1Y2, GYRO1Z1, GYRO1Z2, MAG1X1, MAG1X2, MAG1Y1, MAG1Y2, MAG1Z1, MAG1Z2};
static int CurrentUpdate = ACCEL2X;

GPIO_InitTypeDef GPIO_InitStruct;
void UpdateI2CDAQ(struct FlightDataStruct* state)
{

	state->current_Temperature1=readRegister(I2C1_GYRO_ADDRESS,GYRO_TEMPERATURE_REG);
	float DegreesPerDigit = 0.00875;//8.75 mdps per digit according to l3g4200d specification.
	//float DegreesPerDigit = 500.0f/65536.0f;//calculated out it is 0.00762939... (+-250  with a max range of +-32768)

	int xm =  readRegister(I2C1_GYRO_ADDRESS,0x29);
	int xl =  readRegister(I2C1_GYRO_ADDRESS,0x28);
	state->current_Gyro1_X = (xm<<8)+xl;

	int ym =  readRegister(I2C1_GYRO_ADDRESS,0x2B);
	int yl =  readRegister(I2C1_GYRO_ADDRESS,0x2A);
	state->current_Gyro1_Y = (ym<<8)+yl;

	int zm =  readRegister(I2C1_GYRO_ADDRESS,0x2D);
	int zl =  readRegister(I2C1_GYRO_ADDRESS,0x2C);
	state->current_Gyro1_Z = (zm<<8)+zl;

	state->current_Gyro1_X_DPS = ((double)state->current_Gyro1_X)*DegreesPerDigit;
	state->current_Gyro1_Y_DPS = ((double)state->current_Gyro1_Y)*DegreesPerDigit;
	state->current_Gyro1_Z_DPS = ((double)state->current_Gyro1_Z)*DegreesPerDigit;
	state->current_Gyro1_X_RPS = 0.0174532925f * state->current_Gyro1_X_DPS;
	state->current_Gyro1_Y_RPS = 0.0174532925f * state->current_Gyro1_Y_DPS;
	state->current_Gyro1_Z_RPS = 0.0174532925f * state->current_Gyro1_Z_DPS;

	xm =  readRegister(I2C1_ACCELEROMETER2_ADDRESS,ACCEL2_X_H_A);
	xl =  readRegister(I2C1_ACCELEROMETER2_ADDRESS,ACCEL2_X_L_A);
	state->current_Accel2_X = (xm<<8)+xl;

	ym =  readRegister(I2C1_ACCELEROMETER2_ADDRESS,ACCEL2_Y_H_A);
	yl =  readRegister(I2C1_ACCELEROMETER2_ADDRESS,ACCEL2_Y_L_A);
	state->current_Accel2_Y = (ym<<8)+yl;

	zm =  readRegister(I2C1_ACCELEROMETER2_ADDRESS,ACCEL2_Z_H_A);
	zl =  readRegister(I2C1_ACCELEROMETER2_ADDRESS,ACCEL2_Z_L_A);
	state->current_Accel2_Z = (zm<<8)+zl;

	xm =  readRegister(I2C1_MAGNETOMETER_ADDRESS_WRITE,MAG_X_H_A);
	xl =  readRegister(I2C1_MAGNETOMETER_ADDRESS_WRITE,MAG_X_L_A);
	state->current_Mag1_X = (xm<<8)+xl;
	state->current_Mag1_Xf=state->current_Mag1_X;
	ym =  readRegister(I2C1_MAGNETOMETER_ADDRESS_WRITE,MAG_Y_H_A);
	yl =  readRegister(I2C1_MAGNETOMETER_ADDRESS_WRITE,MAG_Y_L_A);
	state->current_Mag1_Y = (ym<<8)+yl;
	state->current_Mag1_Yf=state->current_Mag1_Y;
	zm =  readRegister(I2C1_MAGNETOMETER_ADDRESS_WRITE,MAG_Z_H_A);
	zl =  readRegister(I2C1_MAGNETOMETER_ADDRESS_WRITE,MAG_Z_L_A);
	state->current_Mag1_Z = (zm<<8)+zl;
	state->current_Mag1_Zf = state->current_Mag1_Z;


}

//doesn't work, but resetting the i2c bus goes something like this. but at 100khz
void reset_I2C(void)
{

	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6; // we want to configure all LED GPIO pins
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; 		// we want the pins to be an output
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; 	// this sets the GPIO modules clock speed
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; 	// this sets the pin type to push / pull (as opposed to open drain)
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOB, &GPIO_InitStruct); 			// this finally passes all the values to the GPIO_Init function which takes care of setting the corresponding bits.
	int i = 0;
	for(;i<1000;i++)
	{
		GPIOB->BSRRL = 0xFFFF;
		int g = 1001;
		for(;g>i;g--)
			;
		GPIOB->BSRRH = 0xFFFF;
	}
}


void init_I2C(void)
{
	init_I2C1(); // initialize I2C peripheral
	init_Gyro();
}


void init_I2C1(void){


	I2C_InitTypeDef I2C_InitStruct;
	NVIC_InitTypeDef NVIC_InitStructure;
	// enable APB1 peripheral clock for I2C1
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
	// enable clock for SCL and SDA pins
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

	reset_I2C();
	/* setup SCL and SDA pins
	 * You can connect I2C1 to two different
	 * pairs of pins:
	 * 1. SCL on PB6 and SDA on PB7
	 * 2. SCL on PB8 and SDA on PB9
	 */
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; // we are going to use PB6 and PB7
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;			// set pins to alternate function
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;		// set GPIO speed
	GPIO_InitStruct.GPIO_OType = GPIO_OType_OD;			// set output to open drain --> the line has to be only pulled low, not driven high
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;			// enable pull up resistors
	GPIO_Init(GPIOB, &GPIO_InitStruct);					// init GPIOB

	// Connect I2C1 pins to AF
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_I2C1);	// SCL
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_I2C1); // SDA

	// configure I2C1
	I2C_InitStruct.I2C_ClockSpeed = 400000; 		// 400kHz
	I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;			// I2C mode
	I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2;	// 50% duty cycle --> standard
	I2C_InitStruct.I2C_OwnAddress1 = 0x00;			// own address, not relevant in master mode
	I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;		// disable acknowledge when reading (can be changed later on)
	I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // set address length to 7 bit addresses

	// enable I2C1

	I2C_Init(I2C1, &I2C_InitStruct);				// init I2C1
	I2C_Cmd(I2C1, ENABLE);

	I2C_GenerateSTOP(I2C1, DISABLE);
}

/* This function issues a start condition and
 * transmits the slave address + R/W bit
 *
 * Parameters:
 * 		I2Cx --> the I2C peripheral e.g. I2C1
 * 		address --> the 7 bit slave address
 * 		direction --> the tranmission direction can be:
 * 						I2C_Direction_Tranmitter for Master transmitter mode
 * 						I2C_Direction_Receiver for Master receiver
 */
void I2C_start(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction){
	// wait until I2C1 is not busy anymore
	while(I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY));

	// Send I2C1 START condition
	I2C_GenerateSTART(I2Cx, ENABLE);

	// wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT));

	// Send slave Address for write
	I2C_Send7bitAddress(I2Cx, address<<1, direction);

	/* wait for I2C1 EV6, check if
	 * either Slave has acknowledged Master transmitter or
	 * Master receiver mode, depending on the transmission
	 * direction
	 */
	if(direction == I2C_Direction_Transmitter){
		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
	}
	else if(direction == I2C_Direction_Receiver){
		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
	}

}

/* This function transmits one byte to the slave device
 * Parameters:
 *		I2Cx --> the I2C peripheral e.g. I2C1
 *		data --> the data byte to be transmitted
 */
void I2C_write(I2C_TypeDef* I2Cx, uint8_t data){

	I2C_SendData(I2Cx, data);
	// wait for I2C1 EV8_2 --> byte has been transmitted
	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

}

/* This function reads one byte from the slave device
 * and acknowledges the byte (requests another byte)
 */
uint8_t I2C_read_ack(I2C_TypeDef* I2Cx){
	// enable acknowledge of recieved data
	I2C_AcknowledgeConfig(I2Cx, ENABLE);
	// wait until one byte has been received
	while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED) );
	// read data from I2C data register and return data byte
	uint8_t data = I2C_ReceiveData(I2Cx);
	return data;
}

/* This function reads one byte from the slave device
 * and doesn't acknowledge the recieved data
 */
uint8_t I2C_read_nack(I2C_TypeDef* I2Cx){
	// disabe acknowledge of received data
	I2C_AcknowledgeConfig(I2Cx, DISABLE);
	// wait until one byte has been received
	while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED) );
	// read data from I2C data register and return data byte
	uint8_t data = I2C_ReceiveData(I2Cx);
	return data;
}

/* This funtion issues a stop condition and therefore
 * releases the bus
 */
void I2C_stop(I2C_TypeDef* I2Cx){
	// Send I2C1 STOP Condition
	I2C_GenerateSTOP(I2Cx, ENABLE);
}

int readRegister(int deviceAddress, char address){
	I2C_start(I2C1, deviceAddress, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
	I2C_write(I2C1, address);
	I2C_stop(I2C1);
	I2C_start(I2C1, deviceAddress, I2C_Direction_Receiver);
	int a = I2C_read_nack(I2C1);
	I2C_stop(I2C1);
    return a;
}


/*
int readRegisterMag(int deviceAddressRead,int deviceAddressWrite, char address){
	I2C_start(I2C1, deviceAddressWrite, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
	I2C_write(I2C1, address);
	I2C_stop(I2C1);
	I2C_start(I2C1, deviceAddressRead, I2C_Direction_Receiver);
	int a = I2C_read_nack(I2C1);
	I2C_stop(I2C1);
    return a;
}
*/
int init_Gyro(void){

	//init the gyro at 250 degrees per second for the full output (65536)
	uint8_t received_data[100];
	I2C_start(I2C1, I2C1_GYRO_ADDRESS, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
	// Enable x, y, z and turn off power down:
	I2C_write(I2C1, GYRO_CTRL_REG1); // write one byte to the slave
	I2C_write(I2C1, 0b01001111);
	//I2C_write(I2C1, 0b00001111); // write another byte to the slave
	// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
	//I2C_write(I2C1, GYRO_CTRL_REG2); // write one byte to the slave
	//I2C_write(I2C1, 0b00101001); // write another byte to the slave
	// Configure CTRL_REG3 to generate data ready interrupt on INT2
	// No interrupts used on INT1, if you'd like to configure INT1
	// or INT2 otherwise, consult the datasheet:
	//I2C_write(I2C1, GYRO_CTRL_REG3); // write one byte to the slave
	//I2C_write(I2C1, 0b00000000); // write another byte to the slave
	// CTRL_REG4 controls the full-scale range, among other things:
	//I2C_write(I2C1, GYRO_CTRL_REG4); // write one byte to the slave
	//I2C_write(I2C1, 0b10110000); // write another byte to the slave
	//I2C_write(I2C1, 0b00010000); // write another byte to the slave
	//I2C_write(I2C1, 0b00110000); // write another byte to the slave
	// CTRL_REG5 controls high-pass filtering of outputs, use it
	// if you'd like:
	//I2C_write(I2C1, GYRO_CTRL_REG5); // write one byte to the slave
	//I2C_write(I2C1, 0b00000000); // write another byte to the slave


	//I2C_write(I2C1, GYRO_FIFO_CTRL_REG);
	//I2C_write(I2C1, 0b00000000);
	I2C_stop(I2C1); // stop setting up the gyro

	//set up the accelerometer2
	I2C_start(I2C1, I2C1_ACCELEROMETER2_ADDRESS, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
	I2C_write(I2C1, ACCEL2_CTRL_REG1); // write one byte to the slave
	I2C_write(I2C1, 0b00101111);
	I2C_stop(I2C1); // stop setting up the Accelerometer

	//set up the magnatometer
	I2C_start(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
	I2C_write(I2C1, MAG_CRA_REG); // write one byte to the slave
	I2C_write(I2C1, 0b00011000);
	I2C_write(I2C1, MAG_GAIN_RE); // write one byte to the slave
	I2C_write(I2C1, 0b00100000);
	I2C_write(I2C1, MAG_MODE_RE); // write one byte to the slave
	I2C_write(I2C1, 0b00011000);
	I2C_stop(I2C1); // stop setting up the Magnetometer
}

void I2C1_EV_IRQHandler(void)
{
  switch(I2C_GetLastEvent(I2C1))
	{
  	  case I2C_EVENT_MASTER_MODE_SELECT:
		  HANDLE_I2C_EVENT_MASTER_MODE_SELECT();break;

	  case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED:
		  HANDLE_I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED();break;

	  case I2C_EVENT_MASTER_BYTE_TRANSMITTING:break;//don't frackin' worry about it.

	  case I2C_EVENT_MASTER_BYTE_RECEIVED:
		  HANDLE_I2C_EVENT_MASTER_BYTE_RECEIVED();break;

		default:
			break;

	}
}

void HANDLE_I2C_EVENT_MASTER_BYTE_RECEIVED()
{

}

void HANDLE_I2C_EVENT_MASTER_MODE_SELECT()
{
	  //at this point we are ready to talk to a slave. Decide what slave to address.


	switch(CurrentUpdate)
	{
	case(ACCEL2X1):I2C_Send7bitAddress(I2C1, I2C1_ACCELEROMETER2_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(ACCEL2X2):I2C_Send7bitAddress(I2C1, I2C1_ACCELEROMETER2_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(ACCEL2Y1):I2C_Send7bitAddress(I2C1, I2C1_ACCELEROMETER2_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(ACCEL2Y2):I2C_Send7bitAddress(I2C1, I2C1_ACCELEROMETER2_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(ACCEL2Z1):I2C_Send7bitAddress(I2C1, I2C1_ACCELEROMETER2_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(ACCEL2Z2):I2C_Send7bitAddress(I2C1, I2C1_ACCELEROMETER2_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(GYRO1X):I2C_Send7bitAddress(I2C1, I2C1_GYRO_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(GYRO1Y):I2C_Send7bitAddress(I2C1, I2C1_GYRO_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(GYRO1Z):I2C_Send7bitAddress(I2C1, I2C1_GYRO_ADDRESS<<1, I2C_Direction_Transmitter);
			break;
	case(MAG1X1):I2C_Send7bitAddress(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE<<1, I2C_Direction_Transmitter);
			break;
	case(MAG1X2):I2C_Send7bitAddress(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE<<1, I2C_Direction_Transmitter);
			break;
	case(MAG1Y1):I2C_Send7bitAddress(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE<<1, I2C_Direction_Transmitter);
			break;
	case(MAG1Y2):I2C_Send7bitAddress(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE<<1, I2C_Direction_Transmitter);
			break;
	case(MAG1Z1):I2C_Send7bitAddress(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE<<1, I2C_Direction_Transmitter);
			break;
	case(MAG1Z2):I2C_Send7bitAddress(I2C1, I2C1_MAGNETOMETER_ADDRESS_WRITE<<1, I2C_Direction_Transmitter);
			break;
	default:
		CurrentUpdate = ACCEL2X;
		HANDLE_I2C_EVENT_MASTER_MODE_SELECT();
	}
}

void HANDLE_I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED()
{
	switch(CurrentUpdate)
	{
	case(ACCEL2X1):I2C_SendData(I2C1, ACCEL2_X_L_A);
			break;
	case(ACCEL2X2):I2C_SendData(I2C1, ACCEL2_X_H_A);
			break;
	case(ACCEL2Y1):I2C_SendData(I2C1, ACCEL2_Y_L_A);
			break;
	case(ACCEL2Y2):I2C_SendData(I2C1, ACCEL2_Y_H_A);
			break;
	case(ACCEL2Z1):I2C_SendData(I2C1, ACCEL2_Z_L_A);
			break;
	case(ACCEL2Z2):I2C_SendData(I2C1, ACCEL2_Z_H_A);
			break;
	case(GYRO1X1):I2C_SendData(I2C1, GYRO_X_L_A);
			break;
	case(GYRO1X2):I2C_SendData(I2C1, GYRO_X_H_A);
			break;
	case(GYRO1Y1):I2C_SendData(I2C1, GYRO_Y_L_A);
			break;
	case(GYRO1Y2):I2C_SendData(I2C1, GYRO_Y_H_A);
			break;
	case(GYRO1Z1):I2C_SendData(I2C1, GYRO_Z_L_A);
			break;
	case(GYRO1Z2):I2C_SendData(I2C1, GYRO_Z_H_A);
			break;
	case(MAG1X1):I2C_SendData(I2C1, MAG_X_L_A);
			break;
	case(MAG1X2):I2C_SendData(I2C1, MAG_X_H_A);
			break;
	case(MAG1Y1):I2C_SendData(I2C1, MAG_Y_L_A);
			break;
	case(MAG1Y2):I2C_SendData(I2C1, MAG_Y_H_A);
			break;
	case(MAG1Z1):I2C_SendData(I2C1, MAG_Z_L_A);
			break;
	case(MAG1Z2):I2C_SendData(I2C1, MAG_Z_H_A);
			break;
	default:
		CurrentUpdate = ACCEL2X;
		I2C1_EV_IRQHandler();
	}

}


